#include "Visualizer.h"
#include <windows.h>
#include <glut.h>
#include "World.h"
#include <ctime>
#include "Vector2.h"
//#include "Track.h"

void init ( GLvoid );
void display ( void );
void arrow_keys ( int a_keys, int x, int y );
void keyboard ( unsigned char key, int x, int y );
void reshape ( int w, int h );
void MouseButton(int button, int state, int x, int y);
void MouseMotion(int x, int y);

DWORD WINAPI glutLoopAsync(LPVOID iValue);

int eyex = 0;
int eyey = 0;
int zoom = 480;
int clickx = 0;
int clicky = 0;
bool button_down = false;

Visualizer::Visualizer(void)
{
}


Visualizer::~Visualizer(void)
{
}

void Visualizer::init(int argc, char* argv[]) {
	glutInit(&argc, argv);
}

void Visualizer::show() {
	HANDLE hThread;
	DWORD dwGenericThread;
	hThread = CreateThread(NULL, 0, glutLoopAsync, NULL, 0, &dwGenericThread);
}

DWORD WINAPI glutLoopAsync(LPVOID iValue) {
	init();
	glutInitDisplayMode ( GLUT_RGB | GLUT_DOUBLE ); // Display Mode
	int size = World::instance()->getSize();
	//glutInitWindowSize  ( size, size ); // If glutFullScreen wasn't called this is the window size
	glutInitWindowSize  ( 720, 720 );
	glutCreateWindow    ( "Grid Lab Visualization" ); // Window Title (argv[0] for current directory as title)
	//glutFullScreen      ( );          // Put Into Full Screen
	glutDisplayFunc     ( display );  // Matching Earlier Functions To Their Counterparts
	glutIdleFunc		( display );
	glutReshapeFunc     ( reshape );
	glutKeyboardFunc    ( keyboard );
	glutMouseFunc (MouseButton);
	glutMotionFunc (MouseMotion);
	glutSpecialFunc     ( arrow_keys );
	glutMainLoop        ( );          // Initialize The Main Loop
	return 0;
}

void init ( GLvoid )     // Create Some Everyday Functions
{
	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
	glClearColor(0.0f, 0.0f, 0.0f, 0.5f);				// Black Background
	glClearDepth(1.0f);									// Depth Buffer Setup
	glEnable(GL_DEPTH_TEST);							// Enables Depth Testing
	glDepthFunc(GL_LEQUAL);								// The Type Of Depth Testing To Do
	glEnable ( GL_COLOR_MATERIAL );
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
	srand((unsigned)time(NULL));
}

#include <iostream>

bool isCleanup(Vector2 & pos){
	//vector<Vector2> cleanupPoints = World::instance()->getCleanup();

	//for(int i = 0; i < cleanupPoints.size(); i ++){
	//	if((int)pos.x == (int)cleanupPoints[i].y &&
	//		(int)pos.y == (int)cleanupPoints[i].x)
	//		return true;
	//}

	return false;
}

bool isCorner(Vector2 & pos) {
	OccupancyProbabilityGrid* grid = World::instance()->occupancy_probability_grid();
	float THRESHOLD = 0.9;

	/*bool tl = false,
		 tm = false,
		 tr = false,
		 ml = false,
		 mr = false,
		 bl = false,
		 bm = false,
		 br = false,
		 ME = false;

	if (grid->at(pos.x, pos.y) > THRESHOLD) {
		ME = true;
	}
	
	if (pos.x > 0 && pos.y > 0) {
		if (grid->at(pos.x - 1, pos.y - 1) > THRESHOLD) {
			tl = true;
		}
	}
	
	if (pos.x > 0) {
		if (grid->at(pos.x - 1, pos.y) > THRESHOLD) {
			ml = true;
		}
	}

	if (pos.x > 0 && pos.y < World::instance()->getSize() - 1) {
		if (grid->at(pos.x - 1, pos.y + 1) > THRESHOLD) {
			bl = true;
		}
	}

	if (pos.y > 0) {
		if (grid->at(pos.x, pos.y - 1) > THRESHOLD) {
			tm = true;
		}
	}

	if (pos.y < World::instance()->getSize() - 1) {
		if (grid->at(pos.x, pos.y + 1) > THRESHOLD) {
			bm = true;
		}
	}

	if (pos.x < World::instance()->getSize() - 1 && pos.y > 0) {
		if (grid->at(pos.x + 1, pos.y - 1) > THRESHOLD) {
			tr = true;
		}
	}

	if (pos.x < World::instance()->getSize() - 1) {
		if (grid->at(pos.x + 1, pos.y) > THRESHOLD) {
			mr = true;
		}
	}

	if (pos.x < World::instance()->getSize() - 1 && pos.y < World::instance()->getSize() - 1) {
		if (grid->at(pos.x + 1, pos.y + 1) > THRESHOLD) {
			br = true;
		}
	}*/

	bool myTemplate[8][25] = {{1,1,1,0,0,
						  1,1,1,0,0,
						  1,1,1,0,0,
						  0,0,0,0,0,
						  0,0,0,0,0},
						{0,0,1,1,1,
						  0,0,1,1,1,
						  0,0,1,1,1,
						  0,0,0,0,0,
						  0,0,0,0,0},
						{0,0,0,0,0,
						  0,0,0,0,0,
						  1,1,1,0,0,
						  1,1,1,0,0,
						  1,1,1,0,0},
						{0,0,0,0,0,
						  0,0,0,0,0,
						  0,0,1,1,1,
						  0,0,1,1,1,
						  0,0,1,1,1},
						{1,1,1,1,1,
						  1,1,1,1,1,
						  1,1,1,1,1,
						  1,1,1,0,0,
						  1,1,1,0,0},
						{1,1,1,1,1,
						  1,1,1,1,1,
						  1,1,1,1,1,
						  0,0,1,1,1,
						  0,0,1,1,1},
						{1,1,1,0,0,
						  1,1,1,0,0,
						  1,1,1,1,1,
						  1,1,1,1,1,
						  1,1,1,1,1},
						{0,0,1,1,1,
						  0,0,1,1,1,
						  1,1,1,1,1,
						  1,1,1,1,1,
						  1,1,1,1,1}};

	int temp[25] = {0,0,0,0,0,
					 0,0,0,0,0,
					 0,0,0,0,0,
					 0,0,0,0,0,
					 0,0,0,0,0};
	
	float THRESHOLD2 = .15;

	int minx = max(pos.x-2, 0)-pos.x,
		maxx = min(pos.x+2, World::instance()->getSize() - 1) - pos.x;

	int miny = max(pos.y-2, 0)-pos.y,
		maxy = min(pos.y+2, World::instance()->getSize() - 1) - pos.y;

	for(int i = minx; i <= maxx; i ++){
		for(int j = miny; j <= maxy; j ++){
			if(i == 0 && j ==0){
				if (grid->at(pos.x, pos.y) > THRESHOLD) {
					temp[12] = 1;
				}
			}
			else{
				if (grid->at(pos.x + i, pos.y + j) > THRESHOLD2) {
					temp[(2+i) * 5 + (2+j)] = 1;
				}
			}
		}
	}

	int count = 0;
	for(int i = 0; i < 8; i ++){
		count = 0;
		for(int j = 0; j < 25; j ++){
			if(temp[j] != myTemplate[i][j])
				count ++;
		}
		if(count < 2)
			return true;
	}

	return false;
}

void display ( void )   // Create The Display Function
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	// Clear Screen And Depth Buffer
	glLoadIdentity();									// Reset The Current Modelview Matrix
	gluLookAt(eyex, eyey, zoom, eyex, eyey, 0, 0, 1, 0);
	
	int size = World::instance()->getSize();
	int mid = size / 2;
	OccupancyProbabilityGrid* grid = World::instance()->occupancy_probability_grid();

	for (int i = 0; i < size; i++) {
		for (int j = 0; j < size; j++) {
			float r = grid->at(i, j);
			if (isCorner(Vector2(i, j))) {
				glColor3f(1, 1, 1);
				glBegin(GL_QUADS);
					glVertex3f(i - mid - 2, j - mid - 2, 5.0f);
					glVertex3f(i + 2 - mid, j - mid - 2, 5.0f);
					glVertex3f(i + 2 - mid, j + 2 - mid, 5.0f);
					glVertex3f(i - mid - 2, j + 2 - mid, 5.0f);
				glEnd();
			}
			else if (isCleanup(Vector2(i, j))) {
				glColor3f(0, 1, 0);
				glBegin(GL_QUADS);
					glVertex3f(i - mid - 2, j - mid - 2, 5.0f);
					glVertex3f(i + 2 - mid, j - mid - 2, 5.0f);
					glVertex3f(i + 2 - mid, j + 2 - mid, 5.0f);
					glVertex3f(i - mid - 2, j + 2 - mid, 5.0f);
				glEnd();
			}
			else{
				glColor3f(r, 0, 0);
				glBegin(GL_QUADS);
				glVertex3f(i - mid, j - mid, 0.0f);
				glVertex3f(i + 1 - mid, j - mid, 0.0f);
				glVertex3f(i + 1 - mid, j + 1 - mid, 0.0f);
				glVertex3f(i - mid, j + 1 - mid, 0.0f);
				glEnd();
			}
		}
	}

	glutSwapBuffers ( );
	// Swap The Buffers To Not Be Left With A Clear Screen
}

void reshape ( int w, int h )   // Create The Reshape Function (the viewport)
{
  glViewport     ( 0, 0, w, h );
  glMatrixMode   ( GL_PROJECTION );  // Select The Projection Matrix
  glLoadIdentity ( );                // Reset The Projection Matrix
  if ( h==0 )  // Calculate The Aspect Ratio Of The Window
     gluPerspective ( 80, ( float ) w, 1.0, 5000.0 );
  else
     gluPerspective ( 80, ( float ) w / ( float ) h, 1.0, 5000.0 );
  glMatrixMode   ( GL_MODELVIEW );  // Select The Model View Matrix
  glLoadIdentity ( );    // Reset The Model View Matrix
}

void MouseButton(int button, int state, int x, int y)
{
	// Respond to mouse button presses.
	// If button1 pressed, mark this state so we know in motion function.
	if (button == GLUT_MIDDLE_BUTTON)
	{
		button_down = (state == GLUT_DOWN);
	}

	clickx = x;
	clicky = y;
}

void MouseMotion(int x, int y)
{
	if (button_down) {
		zoom += (y - clicky);
	}
	else {
		eyex += (clickx - x);
		eyey += (y - clicky);
	}

	clickx = x;
	clicky = y;
}

void keyboard ( unsigned char key, int x, int y )  // Create Keyboard Function
{
  switch ( key ) {
    case 27:        // When Escape Is Pressed...
      exit ( 0 );   // Exit The Program
      break;        // Ready For Next Case
    default:        // Now Wrap It Up
      break;
  }
}

void arrow_keys ( int a_keys, int x, int y )  // Create Special Function (required for arrow keys)
{
  switch ( a_keys ) {
    case GLUT_KEY_UP:     // When Up Arrow Is Pressed...
      glutFullScreen ( ); // Go Into Full Screen Mode
      break;
    case GLUT_KEY_DOWN:               // When Down Arrow Is Pressed...
      glutReshapeWindow ( 500, 500 ); // Go Into A 500 By 500 Window
      break;
    default:
      break;
  }
}